#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import rospy
import moveit_commander
import geometry_msgs.msg
import eye_in_hand_zc3_all
import force_control_all
import subprocess

def start_force_sensor():
    # 启动力传感器节点
    sensor_process = subprocess.Popen(['python3', '/home/jetson/zc3/src/visual_control/src/force_run3.py'])
    print("力传感器节点打开")
    return sensor_process

def move_to_multiple_targets():
    # 初始化 moveit_commander 和 rospy
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface', anonymous=True)

    # 初始化机器人手臂的 move group
    arm_group = moveit_commander.MoveGroupCommander("manipulator")

    # 获取初始位置
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 清空之前的目标
    arm_group.clear_pose_targets()

    # 定义多个目标位姿
    waypoints = []

    # 第1个目标位姿 0/-55/90/20/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = -0.017099604521405784
    pose1.position.y = -2.8315387428728567e-05
    pose1.position.z = 0.11438909247802503
    pose1.orientation.x = -3.453025354057534e-05
    pose1.orientation.y = 0.46175405203633646
    pose1.orientation.z = -7.30327633778306e-05
    pose1.orientation.w = 0.887007998217548
    waypoints.append(pose1)

    # 第2个目标位姿 0/-58/115/-3/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.004651029454941641
    pose2.position.y = -2.046464646042786e-06
    pose2.position.z = 0.07989258904926122
    pose2.orientation.x = -3.9551522751682376e-05
    pose2.orientation.y = 0.4523160885130879
    pose2.orientation.z = 6.4712743132175465e-06
    pose2.orientation.w = 0.8918576985517478
    waypoints.append(pose2)

    # 第3个目标位姿 0/-80/131/-11/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = 0.016710764091943137
    pose3.position.y = 4.7481613370422367e-05
    pose3.position.z = 0.06725314521298217
    pose3.orientation.x = 2.484310933627909e-06
    pose3.orientation.y = 0.34210428530462056
    pose3.orientation.z = 0.00014130528033512744
    pose3.orientation.w = 0.9396619807158642
    waypoints.append(pose3)

    # 4 0/-90/142/-26/0
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.061313835276576675
    pose4.position.y = 3.4229349294604146e-05
    pose4.position.z = 0.09607582904327464
    pose4.orientation.x = 8.189114554395947e-06
    pose4.orientation.y = 0.19079166522506064
    pose4.orientation.z = 9.397613957952172e-05
    pose4.orientation.w = 0.9816305473891067
    waypoints.append(pose4)

    # 5 0/-90/146/-32/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.06469858300599692
    pose5.position.y = 6.72000076315633e-05
    pose5.position.z = 0.07814980502181258
    pose5.orientation.x = -3.84250569528825e-06
    pose5.orientation.y = 0.20806965601170563
    pose5.orientation.z = 0.00018064511789783727
    pose5.orientation.w = 0.978113994174374
    waypoints.append(pose5)

    # 第7个目标位姿 0/-90/155/-65/0 举平new
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.10862679372272958
    pose7.position.y = 7.322812585541887e-05
    pose7.position.z = 0.151673237887705
    pose7.orientation.x = 1.843833939518914e-06
    pose7.orientation.y = 0.0002691494452088297
    pose7.orientation.z = 0.00018408295291220934
    pose7.orientation.w = 0.9999999468343201
    waypoints.append(pose7)

    # 第8个目标位姿 -15/-90/155/-68/0
    pose8 = geometry_msgs.msg.Pose()
    pose8.position.x = 0.10150451167124033
    pose8.position.y = -0.059668682905588945
    pose8.position.z = 0.1647479924791181 #0.15
    pose8.orientation.x = -0.003413749283397351
    pose8.orientation.y = -0.02598662685755732
    pose8.orientation.z = -0.1307826110962234
    pose8.orientation.w = 0.9910645539899258
    waypoints.append(pose8)

    # 第10个目标位姿 
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.12119300384217499
    pose10.position.y = -0.06496064015101635
    pose10.position.z = 0.16648646107426138
    pose10.orientation.x = -0.003406584361246141
    pose10.orientation.y = -0.02590040792578116
    pose10.orientation.z = -0.130787996023333
    pose10.orientation.w = 0.9910661250130935
    waypoints.append(pose10)

    # 第11个目标位姿 -15/-75/135/-68/0
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.10021590416090176
    pose11.position.y = -0.059275851310574126
    pose11.position.z = 0.24717377948372102
    pose11.orientation.x = -0.009100789371875279
    pose11.orientation.y = -0.06914916681497274
    pose11.orientation.z = -0.1303713558152669
    pose11.orientation.w = 0.9890090383532868
    waypoints.append(pose11)

    # 第12个目标位姿 0/-75/135/-68/0
    pose12 = geometry_msgs.msg.Pose()
    pose12.position.x = 0.1080342010592359
    pose12.position.y = -6.057566946212703e-06
    pose12.position.z = 0.24717779672627277
    pose12.orientation.x = 9.786502587199877e-06
    pose12.orientation.y = -0.06972668749366641
    pose12.orientation.z = -2.98995522507148e-05
    pose12.orientation.w = 0.9975661321744048
    waypoints.append(pose12)

    # 第14个目标位姿 0/-70/90/-20 设定这个位置为初始位置
    pose14 = geometry_msgs.msg.Pose()
    pose14.position.x = 0.017405918539271842
    pose14.position.y = -1.2171559032328237e-06
    pose14.position.z = 0.30727696933440546
    pose14.orientation.x = 1.829736299180823e-06
    pose14.orientation.y = 8.08447651258564e-05
    pose14.orientation.z = 6.071146226902279e-06
    pose14.orientation.w = 0.9999999967119587
    waypoints.append(pose14)


    # 规划并执行笛卡尔轨迹
    (plan, fraction) = arm_group.compute_cartesian_path(
        waypoints,   # 目标位姿序列
        0.01,        # eef_step，终端执行器的步长
        False)         # jump_threshold，跳跃阈值
    print("Fraction of path achieved: ", fraction)

    # 执行轨迹
    arm_group.execute(plan, wait=True)

    # 控制机械臂先回到初始化位置
    arm_group.set_named_target('middle')
    arm_group.go()
    rospy.sleep(1)

    # 调用视觉处理代码
    eye_in_hand_zc3_all.vision_processing()

    # 停止运动
    arm_group.stop()

    # 清空之前的目标
    arm_group.clear_pose_targets()

    # 启动力传感器节点
    # sensor_process = start_force_sensor()
    # start_force_sensor()

    rospy.sleep(8)
    # print("延时进入力控……")
    # 调用力控代码
    force_control_all.force_control_main()

    # 关闭并退出 moveit_commander
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

if __name__ == '__main__':
    try:
        move_to_multiple_targets()
    except rospy.ROSInterruptException:
        pass
